//////////////////////////////////////////////////////////////////////////
// triface - class to hold information about triangular face
// By Antonio Miranda - October 2010
//////////////////////////////////////////////////////////////////////////


#include <math.h>
#include "triface.h"
#include "node.h"

#define PI                              3.141592653589793324
#define ABOUT_ZERO(val,tol)             ((val<tol)&&(val>(-tol)))
#define OUT_FACE(val,tol)               ((val>(1.0+tol))||(val<(0.0-tol)))
#define MAX(a,b)                        (((a)>(b))?(a):(b))
#define MIN(a,b)                        (((a)<(b))?(a):(b))
#define SIGN(dot)                       (((dot)>0.0)?(1):(-1))

//
//////////////////////////////////////////////////////////////////////////
triface::triface(void)
{
  vert[0] = -1;
  vert[1] = -1;
  vert[2] = -1;

  // adj elements
  adjElem[0] = -1;
  adjElem[1] = -1;

  // attributes
  double area = 0;

  // flags
  use   = 0;
  key   = -1;
  layer = 0;
  frac  = 0;

  boundary = true; // all faces start as boundary

  removed = false;
}

//
//////////////////////////////////////////////////////////////////////////
triface::~triface(void)
{
}



//
//////////////////////////////////////////////////////////////////////////
void triface::addVertex( int i, int j, int k, vector <node *> &nodes)
{

  double *ip, *jp, *kp;

  ip = nodes[i]->coord;
  jp = nodes[j]->coord;
  kp = nodes[k]->coord;

  vert[0] = i;
  vert[1] = j;
  vert[2] = k;

  sort();
  
  center[0] = (ip[0] + jp[0] + kp[0]) / 3.0; 
  center[1] = (ip[1] + jp[1] + kp[1]) / 3.0; 
  center[2] = (ip[2] + jp[2] + kp[2]) / 3.0;

  box.clear();
  box.update(ip[0], ip[1], ip[2]);
  box.update(jp[0], jp[1], jp[2]);
  box.update(kp[0], kp[1], kp[2]);

  double u[3], v[3];
  for (int m = 0; m < 3; ++m)
  {
    u[m] = jp[m] - ip[m];
    v[m] = kp[m] - ip[m];
  }

  // compute normal. Invert to point to inside body
  r_x_s[0] = (u[1]*v[2] - u[2]*v[1]);
  r_x_s[1] = (u[2]*v[0] - u[0]*v[2]);
  r_x_s[2] = (u[0]*v[1] - u[1]*v[0]);
  double dot = sqrt (r_x_s[0]*r_x_s[0] + r_x_s[1]*r_x_s[1] + r_x_s[2]*r_x_s[2]);

  area = dot * 0.5;

  //if (dot > 0)
  //{
  //  normal[0] /= dot;
  //  normal[1] /= dot;
  //  normal[2] /= dot;
  //}

  //cout << "Add Face = " << vert[0] << ", " << vert[1] << ", " << vert[2] << endl;
}


//
//////////////////////////////////////////////////////////////////////////
void triface::getBox( double &xmin, double &ymin, double &zmin,
                      double &xmax, double &ymax, double &zmax)
{
  box.get(xmin, ymin, zmin, xmax, ymax, zmax);
}

//
//////////////////////////////////////////////////////////////////////////
double triface::distToPoint( double pts[3] )
{
  double vec[3] = {pts[0]-center[0], pts[1]-center[1], pts[2]-center[2]};
  return (r_x_s[0]*vec[0] + r_x_s[1]*vec[1] + r_x_s[2]*vec[2]);
}

//
//////////////////////////////////////////////////////////////////////////
void triface::sort()
{
  if (vert[0]>=vert[1] && vert[0]>=vert[2])
  {
    sort_vert[2] = vert[0];
    if (vert[1]>=vert[2])
    {
      sort_vert[1] = vert[1];
      sort_vert[0] = vert[2];
    }
    else 
    {
      sort_vert[1] = vert[2];
      sort_vert[0] = vert[1];
    }
  }

  else if(vert[1]>=vert[0] && vert[1]>=vert[2])
  {
    sort_vert[2] = vert[1];
    if (vert[0]>=vert[2])
    {
      sort_vert[1] = vert[0];
      sort_vert[0] = vert[2];
    }
    else 
    {
      sort_vert[1] = vert[2];
      sort_vert[0] = vert[0];
    }
  }

  else
  {
    sort_vert[2] = vert[2];
    if (vert[0]>=vert[1])
    {
      sort_vert[1] = vert[0];
      sort_vert[0] = vert[1];
    }
    else 
    {
      sort_vert[1] = vert[1];
      sort_vert[0] = vert[0];
    }
  }
}

//
//////////////////////////////////////////////////////////////////////////
double triface::solidAngle( double cen[3], vector <node *> &nodes )
{
  int                i, n_vert;
  double             *verts[3], *p1, *p2;
  double             area = 0.0, mod, dot ;
  double             r[3], s[3], p[3], r_x_s[3], a[3], r1[3], b[3] ;
  double             edge, height, dist;

  // put the face vertices in a polygon structure
  n_vert = 3 ;
  verts[0] = nodes[vert[0]]->coord ; 
  verts[1] = nodes[vert[1]]->coord ; 
  verts[2] = nodes[vert[2]]->coord ; 

  /* find the normal of the polygon */
  r_x_s[0] = r_x_s[1] = r_x_s[2] = 0.0 ;
  r[0] = verts[1][0]-verts[0][0] ;
  r[1] = verts[1][1]-verts[0][1] ;
  r[2] = verts[1][2]-verts[0][2] ;
  for ( i = 2; i < n_vert ; i++ )
  {
    s[0] = verts[i][0]-verts[0][0] ;
    s[1] = verts[i][1]-verts[0][1] ;
    s[2] = verts[i][2]-verts[0][2] ;
    p[0] = r[1] * s[2] - s[1] * r[2] ;
    p[1] = r[2] * s[0] - s[2] * r[0] ;
    p[2] = r[0] * s[1] - s[0] * r[1] ;
    r_x_s[0] += p[0] ;
    r_x_s[1] += p[1] ;
    r_x_s[2] += p[2] ;
    r[0] = s[0] ;
    r[1] = s[1] ;
    r[2] = s[2] ;
  }
  mod = sqrt( r_x_s[0]*r_x_s[0] + r_x_s[1]*r_x_s[1] + r_x_s[2]*r_x_s[2] ) ;
  r_x_s[0] /= mod ;
  r_x_s[1] /= mod ;
  r_x_s[2] /= mod ;

  // verify if the index vertex is too near from the polygon plane
  edge = sqrt( (4*area) / sqrt(3.0) );
  height = (edge * sqrt(13.0)) / 4.0;

  dist = fabs (distToPoint(cen));
  if ( dist <= (height/(10.0*1.0)) )
    return 0.0;

  // find the angle solid that the index vertex makes with the polygon

  p2 = verts[n_vert-1] ;  /* last  vertex */
  p1 = verts[0] ;         /* first vertex */
  a[0] = p2[0]-p1[0], a[1] = p2[1]-p1[1], a[2] = p2[2]-p1[2] ; 

  for ( i = 0; i < n_vert; i++ ) 
  {
    r1[0] = p1[0]-cen[0], r1[1] = p1[1]-cen[1], r1[2] = p1[2]-cen[2] ;
    p2 = verts[(i+1)%n_vert] ;
    b[0] = p2[0] - p1[0], b[1] = p2[1] - p1[1], b[2] = p2[2] - p1[2] ;

    area += solidEdge( a, b, r1, r_x_s ) ; 

    a[0] = -1.0*b[0] ; a[1] = -1.0*b[1] ; a[2] = -1.0*b[2] ;
    p1 = p2 ;
  }

  area -= PI*(n_vert-2) ;
  dot = ( r_x_s[0] * r1[0] + r_x_s[1] * r1[1] + r_x_s[2] * r1[2] ) ;
  return (dot>0.0) ? (-area) : (area) ;
}

//
//////////////////////////////////////////////////////////////////////////
double triface::solidEdge( double a[3], double b[3], double r1[3], double r_x_s[3] )
{
  double              n1[3], n2[3], l1, l2, s, ang, ba[3] ;

  n1[0] = a[1] * r1[2] - r1[1] * a[2] ;
  n1[1] = a[2] * r1[0] - r1[2] * a[0] ;
  n1[2] = a[0] * r1[1] - r1[0] * a[1] ;
  n2[0] = r1[1] * b[2] - b[1] * r1[2] ;
  n2[1] = r1[2] * b[0] - b[2] * r1[0] ;
  n2[2] = r1[0] * b[1] - b[0] * r1[1] ;
  l1 = sqrt( n1[0] * n1[0] + n1[1] * n1[1] + n1[2] * n1[2] ) ;
  l2 = sqrt( n2[0] * n2[0] + n2[1] * n2[1] + n2[2] * n2[2] ) ;
  s = (( n1[0] * n2[0] + n1[1] * n2[1] + n1[2] * n2[2] ) / ( l1 * l2 )) ;
  ang = acos(MAX(-1.0,MIN(1.0,s))) ;
  ba[0] = b[1] * a[2] - a[1] * b[2] ;
  ba[1] = b[2] * a[0] - a[2] * b[0] ;
  ba[2] = b[0] * a[1] - a[0] * b[1] ;
  s = ( ba[0] * r_x_s[0] + ba[1] * r_x_s[1] + ba[2] * r_x_s[2] ) ;
  return (s>0.0) ? (PI-ang) : (PI+ang) ;
}

//
//////////////////////////////////////////////////////////////////////////
double triface::volume( double pts[3], vector <node *> &node_list)
{
  double             ind_vol = 0.0 ;

  /* compute the volume of tetrahedron */
  ind_vol += volumeFace (node_list[vert[0]]->coord, 
                         node_list[vert[2]]->coord,
                         node_list[vert[1]]->coord) ;

  ind_vol += volumeFace(node_list[vert[0]]->coord, 
                        node_list[vert[1]]->coord,
                        pts) ;

  ind_vol += volumeFace(node_list[vert[1]]->coord, 
                        node_list[vert[2]]->coord,
                        pts) ;

  ind_vol += volumeFace(node_list[vert[2]]->coord, 
                        node_list[vert[0]]->coord,
                        pts) ;

  /* return the volume of tetrahedron */
  return ind_vol ;
}

//
//////////////////////////////////////////////////////////////////////////
double triface::volumeFace( double p1[3], double p2[3], double p3[3] )
{
  double p4[3] = {p2[1]*p3[2]-p2[2]*p3[1], p2[2]*p3[0]-p2[0]*p3[2], p2[0]*p3[1]-p2[1]*p3[0]};
  return (p1[0]*p4[0] + p1[1]*p4[1] + p1[2]*p4[2]) / 6.0;
}

//
//////////////////////////////////////////////////////////////////////////
double triface::Srms( double pts[3], vector <node *> &nodes )
{
  // vertex coordinates
  double *verts[3];
  verts[0] = nodes[vert[0]]->coord; 
  verts[1] = nodes[vert[1]]->coord; 
  verts[2] = nodes[vert[2]]->coord; 

  double ind_rms = 0.0 ;

  // compute the rms of tetrahedron
  ind_rms += SrmsEdge (verts[0], verts[1]);
  ind_rms += SrmsEdge (verts[0], verts[2]);
  ind_rms += SrmsEdge (verts[1], verts[2]);

  ind_rms += SrmsEdge (verts[0], pts);
  ind_rms += SrmsEdge (verts[1], pts);
  ind_rms += SrmsEdge (verts[2], pts);

  ind_rms = sqrt( ind_rms ) ;

  // return the rms of tetrahedron
  return ind_rms;
}

//
//////////////////////////////////////////////////////////////////////////
double triface::SrmsEdge( double v1[3], double v2[3] )
{
  int                i ;
  double             edg_rms, len, vec[3] ;

  // get the edge vector
  for( i = 0; i < 3; i++ )
    vec[i] = v1[i] - v2[i];

  // compute the length for the edge
  len = sqrt (vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);

  // compute the rms contribution for the edge
  edg_rms = ((1/6.0) * len * len) ;

  return edg_rms ;
}

//
//////////////////////////////////////////////////////////////////////////
bool triface::isPointAbove( double pts[3] )
{
  return (distToPoint(pts) > 0? true : false);
}

//
//////////////////////////////////////////////////////////////////////////
bool triface::isPointBellow( double pts[3] )
{
  return (distToPoint(pts) < 0? true : false);
}

//
//////////////////////////////////////////////////////////////////////////
void triface::print()
{
  cout << "use = " << use << ", key = " << key << ", area = " << area << endl;
  cout << "Center = " << center[0] << ", " << center[1] << ", " << center[2] << endl;
  cout << "Normal = " << r_x_s[0] << ", " << r_x_s[1] << ", " << r_x_s[2] << endl;
}

//
//////////////////////////////////////////////////////////////////////////
void triface::printNodes()
{
  cout << "Face V = " << vert[0] << ", " << vert[1] << ", " << vert[2] << endl;
}